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ABSTRACT 


In the tracking of a moving ground target by small unmanned air vehicle 
(UAV) via camera vision, the target position and motion cannot be measured 
directly. Two different types of filters were assessed for their ability to estimate 
target motion, namely target velocity, directional heading on flat ground and 
distance from the UAV to target. The first filter is a nonlinear deterministic filter 
with stability guarantee. The second filter is based on nonlinear Kalman Filter 
technique. The application and performance of these two filters are presented, 


for simulated vision based target tracking. 
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I. INTRODUCTION 


A. BACKGROUND 

The goal of this project was to enhance the target tracking features of the 
airborne sensor in support of the Tactical Network Topology (TNT) experiment, in 
which the Naval Postgraduate School (NPS) is participating. This experiment 
assesses the information flow in a network through scenario plays and gathers 
part of the required information through various sensors. The airborne sensor is 
one such sensor through which ground target information can be collected. 
Currently the airborne sensor includes the Small Unmanned Aerial Vehicle 
(SUAV) equipped with a pan-tilt camera for target tracking purpose. This sensor 
was previously developed in NPS as a system that incorporates ground target 
tracking control and SUAV guidance. The SUAV to target distance information 
was used to guide the SUAV to fly in a circular path, to facilitate continuous 
tracking by its onboard camera. The current target tracking process focuses on a 
stationary ground target and is able to estimate the range from the SUAV to the 
target. In the case of a moving ground target, the current tracking process does 
not yield information on the speed and direction which the target is traveling. 
B. PROBLEM FORMULATION 

The purpose of this project was to investigate the use of a filter to estimate 
the ground target speed and heading. In this thesis, the applications of two 
different filters were discussed, with regards to the formulation of the filter and 


also the filter performance in tracking motion. 


In order to assess the filter performance, existing SUAV truth models were 
used to provide flight and camera models. The main focus of this project was the 
estimation of target speed and the range from SUAV to target. The range is an 
important variable to be estimated, as this has a bearing on the guidance for the 
SUAV flight pattern. 
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ll. LITERATURE SURVEY 


A. RANGE ESTIMATION FOR STATIONARY TARGET 

The study and design of the control system using SUAV and onboard 
vision device for tracking stationary target was previously carried out in NPS. In 
this section that follows, the technique of range estimation by Prince [Ref 1] is 


summarized. 


The range from the SUAV to a ground target was estimated using linear 
discrete Kalman Filter. The range was subsequently used to guide the SUAV to 
fly in a circular path around the target, so that the SUAV can maintain a defined 
distance from the target during the tracking process. The Kalman filtering 
technique was employed to estimate the range from SUAV to target and the 


filter’s system equation was given by 


Xia = PX, + Wy [Ref 1] 


1 At 

where x, -|°*| : F=| | and w, ~N(0,Q,) is the process noise with 
Pr k 

covariance Q,, At denotes the sample time and p denotes the range from SUAV 


to the target. The measurement equation was given by 


Z, = HX, +v, [Ref 1] 


P 7 

where Z, "| y LAS fe, 0 and v, ~N(0,R,) is the measurement noise 
Pk \y 0 1 

with covariance Fx, ye denotes the estimated line-of-sight (LOS) rate and 

V,’ denotes the SUAV velocity vector that is perpendicular to, the LOS to target, 


where V,° = pi. The estimated line-of-sight rate 4, was obtained using another 


set of system and measurement equations, see Prince [Ref 1] for more details. 


Constant Kalman gains Kx; could be used in the measurement update 
equation to obtain the updated range estimate. The measurement update was 


given by 
Xa = Xt + Ket (Zr — Apt Xa) [Ref 1] 


This application of Kalman filter to this problem was successful in 
obtaining the estimated range thus enabling the use of range information for 
SUAV flight path control and for determining target location. 


Another approach to estimate the range to a stationary target was done by 
Wang et al [Ref 2] using nonlinear Kalman filtering. The following diagram shows 
the definitions of key variables used in the tracking kinematics. 


SUAV 





target 


Figure 1. Moving target tracking in inertial X; and Y; frame [After: Ref 2]. 


The process model was given by 


[Ref 2] 


1 —A+y 


where x=| p|, f(x,w)= —V, sing ‘ 
Z soe y: oe 
Pp Pp 
Y | 7 1 
Ze = and mx) =| \ [Ref 2] 
i ly | pa 


A series of steady state Kalman gains K was computed based on several range 
p and estimation of the range to the moving target was obtained in simulation. In 
the same study, the theoretical range was obtained through derivation of 
kinematics relationship between SUAV and target. 
B. VELOCITY ESTIMATION OF UNDERWATER VEHICLE 

In separate study by Oliveira et al [Ref 3], an Autonomous Surface Craft 
(ASC) tracked the velocity of an Autonomous Underwater Vehicle (AUV) and 
estimated the velocity of the later using a nonlinear estimator. The nonlinear 
relationship of this tracking problem was solved based on the theory of linear 
parametrically varying system. This section describes the study by Oliveira et al 
[Ref 3]. 


There are two parts to the solution, a process model and a tracker design. 
The process model comes in the form: 


p=-gR(A)S('Vs)m_ ++ W, 

b=0 [Ref 3] 

Ym = h,(°p) + W, 
where p was the position of the AUV, b was the velocity bias to be estimated. 
The rotation matrix transforming {S} to {I} frame is JA(A) and w is the noise 
input. The measurement ym is given by y=|u, v, Zz)’, h,(°p) was the 
mapping of the position of AUV, with respect to camera frame, into the camera 
image plane Uc, ve and vertical height z from ASC to AUV. The figure below 
shows the relationship of ASC/camera and AUV, in relation to the inertial frame. 
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ASC: {S} 


Inertial 
frame: {I} 





AUV: {U} 


Figure 2. Tracking of underwater AUV target [After: Ref 3]. 


The key to estimating the AUV velocity is the relationship: 
h,(°p)—h, (p) = L(“B,° p)H(°P)(°P-“p) [Ref 3] 
where L(°p,°p) = diag(Z,/Zo;Z¢/Zo:1)and H(°p) is the Jacobian of h,(°p). By 


having L(°p,°p) =! if 2,/Z> ~1, the expression becomes 


[Ref 3] (1) 


This expression relates errors in sensor measurement to errors in the 


estimation variables, thus casting the estimation problem in linear parametrically 
varying system (LPV) framework. The filter realization is given by: 


P=-gR(A)>('V5)m + B+ Ky-g RAYA” (°B)(h, (OB) - Yn) 
[Ref 3] 
b= K,-¢ R(A)H '(°B)(h,(B) — Ym) 


This filter realization is also shown in figure 3. 





Figure 3. Tracker structure [After: Ref 3]. 


Estimator gains K; and Kz were selected to achieve desired performance 
for the filter. 
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lll. PROBLEM FORMULATION 


A. PROBLEM FORMULATION 

In the subsequent chapters, two approaches to estimation of range and 
velocity of the moving ground target are described. The first approach uses the 
filter based on LPV system as described in chapter Il. The second approach 
attempts to estimate the target range and velocity based on continuous nonlinear 
Kalman filtering using steady state Kalman gains. Both approaches will be 
assessed through simulations using the MATLAB tool. 

1. Nonlinear Deterministic Filter with Stability Guarantee 

The following diagram shows the framework for SUAV and target in the 


inertial frame, which will be used in the subsequent velocity estimation. 


Velocity: 
SUAV (V9) Camera 
tracking error 
represented 
by Uc, Ue on 
camera image 
plane 








Xi LOS 


{1} 
Y, @ 


Target 


Figure 4. SUAV tracking a moving target in inertial frame. 


The approach to estimate target range and velocity requires measurement 
inputs from the tracking errors Uc and vg, as well as altitude z and SUAV velocity. 
Altitude was assumed to be obtained from the SUAV altimeter. The velocity was 
assumed to be obtainable from GPS based computation. 


For the purpose of this study, an already existing UAV truth model was 
used to provide the required inputs to the filter. The filter was assessed for its 
ability to track the target with and without the addition of measurement noise. 

2. Filter Based on Kalman Filtering Technique 

The following diagram shows the framework for SUAV and target motion 
in the inertial frame. Here the framework is two dimensional as compared to the 


previous filter approach which was three dimensional. 





A Velocity: 


(Vo) 


Velocity: 
(V1) 
Target 
(1) Y, 
Z 
up 


Figure 5. Target tracking framework for Kalman filter approach. 


The Kalman filter approach required collection of measurements for 
velocity V, and angle 7. These measurements were compared with the 


estimated V, and 7. Angle 7 is the angle between the vector Vp, which is 


perpendicular to the LOS, and the velocity vector Vs. These values were 
assumed to be obtainable from the SUAV flight data. An already existing UAV 
truth model was used to provide the necessary inputs to the filter. Together with 
the process model, which will be discussed in more detail, the computed Kalman 


gain was used to generate new estimates of the state variables. 


B. COORDINATE SYSTEM 

1. Camera Coordinates 

The camera frame is denoted by {C} and has coordinates Xc, Yc and Z, 
where X, is the distance to the target and has its origin at the camera pin-hole 
location. X, is positive in the direction of the target, along the camera to target 
axis. Y, is the lateral distance of the target from the X, axis and Z, is the vertical 
distance of the target, positive when pointing downwards from the X, axis. The Y. 


and Z, position of the target will be represented as the u, and v. offset distance 


from the X_ axis respectively, on the camera image plane. The focal length f of 
the lens is 12mm. Finally the camera is located at a height Z from the target, in 


the inertial frame. The camera coordinate system is illustrated below. 


Focal te 
Camera length 


origin 


Camera 
J image plane 















ail 
Target at Xo, Yo, Ze 


Figure 6. Camera coordinate system. 


2. Gimbal Coordinates 
The camera pointing angles or the gimbal angles are denoted by two 


angles namely gimbal pitch 0, (or tilt angle) and yaw g. (or pan angle). These 


are angled with respect to the SUAV airframe body coordinate system. 
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3. Body Coordinates 


The SUAV airframe body frame is denoted by {B} and has coordinates Xs 


pointing towards the nose of the SUAV, Ys pointing to right wing of SUAV and Zp 


pointing upwards from the SUAV. The airframe is positioned in the inertial frame 


and rotated by the angles roll ¢,, pitch 6, and yaw g,. These are computed with 


respect to the inertial frame. 


4. Inertial Coordinates 


The inertial coordinate system is denoted by {I} and has coordinates Xi, Y| 


and Z). 
5. Transformation Matrix 
The rotation matrix from camera to body frame [Ref 5] is: 
cos@,cosg, cosd,sing, -sind, 

°"R=| -sing, cose, 0 


sING, sing, sin@, cosd, 


The rotation matrix from body to inertial frame [Ref 5] is: 


COS @, COS Mp COS 0, SING, 


,R=|cosg, sind, sing, —SinggCOS¢, COSy,cos¢, + sing, sind, sing, 
cos, Sind, sing, +Sing,COS¢, Sing, SiINO, COS¢, —COS@, Sind, 


—sin6O, 
cos 6, sing, 
COS 0, COS dp 


Then, the rotation matrices from camera to inertial frame and vice-versa are: 


cR=,R-éR and 


CR=(¢R)" respectively. 
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IV. APPLICATION OF NONLINEAR DETERMINISTIC FILTER 
WITH STABILITY GUARANTEE 


A. DESCRIPTION OF FILTER 

The filter described in Oliveira et al [Ref 3] was applied to the moving 
ground target tracking problem in this project. Instead of tracking the AUV, the 
filter technique was used to track the ground target. A six DOF SUAV truth model 
from Lizarraga [Ref 8] was used to generate required inputs for the filter. The 
following sections describe the filter in more detail. 

1. Process Model 

The following process model in equations (2) to (4) [Ref 3] was used, in 
absence of noise, to implement the SUAV to ground target tracking: 


P= H'Vo)m+0 (2) 
b=0 (3) 
Vm =h,(°p) (4) 


In equation (1) above, the first term —(‘v refers to the inertial speed of 


ae 
SUAV. The second term b denotes the actual target velocity which the filter will 
estimate. Hence the estimated velocity of the target V, will be based on b. 
Based on the assumption that the target is traveling at a constant speed and 
heading, the time derivative b is zero in equation (3). The first term in the 
measurement equation (4) converts the position of target in camera coordinates 
Cp, i.€., Xc, Ye and Zz into the image plane coordinates and altitude difference z, , 


according to the relationship: 














f-y, 
=“ ee 
V=\o01= 7 [Ref 3] (5) 
21 Pyy Xo + Pog Vo + Ag + Z, 


where fis 12mm, Ry2, Re3 and A33 are the elements of the third column of the 
rotation matrix °R. Equation (5) represents the measurement equation 
Ym =h,(°p)- 

The process model was then used to design the filter following Oliveira et 
al [Ref 3] with minor modification: 
P= —('Vg)m + b+ Ky-g RH" (°B)(h, (OB) — Ym) (6) 


b= Ky-R-H"(°B)(h, (°B) - Ym) (7) 


The notation H(°p) refers to the Jacobian of h,(°p) and H™'(°p) is the 
inverse of H(°p). The selection of gains K; and K2 will be described in the next 


section. The resulting implementation is shown below: 





Tracker structure [After: Ref 3]. 


Figure 7. 


2 Gain Selection 
Using identity (1) and assuming x, = x,, the filter dynamics are given by 





Figure 8. Basic position model. 


By implementing the observer according to Ogata [Ref 7], which has the form 


A ‘ 0 / 0 K. 

x = Ax+Bu+L(y,, —Cx), where A= , B= ,C=[/ O]., L=| "| with 
0 0 | K, 

K; and Kz being the gains shown in the filter structure, K; and Kz can be found by 

pole placement technique as described in Ogata [Ref 7]. The poles selected 


were [-3 -3 -3 -1 -1 -1] and the resulting gains were: 


-4 0 0 =O. 0 
K,=| 0 -4 0] and K,=|0 -3 0 
0 O -4 0 0 -3 


These were the initial gains used to assess the filter performance. The gains 
were subsequently varied to address noise. 
B. RESULTS AND DISCUSSION 

1. Filter Performance in Absence of Noise 

The following graphs show the performance of the filter wnen the SUAV 
truth model was configured such that the SUAV attempts to circle around a 
moving target traveling at velocity [10, 5, 0] m/s in the inertial frame, along xi, yi 


and z; axis. 


The results showed that the convergence of estimated target velocity was 


achieved in five seconds in absence of measurement noise. 


Velocity of target in X direction 
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Figure 9. 
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Position error of target in X direction 
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Figure 10. Target position error in inertial frame. 
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Estimated range from UAV to target 
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Figure 11. 
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Target heading 














Figure 12. Estimated target heading. 


The target heading was computed from the angle resulting from the vector 
summation of the estimated target velocity components along x; and y; axis. The 
estimated target heading corresponded well with the true target heading of 26.6 


degrees. 


By reducing the gains kK; and Kg, it was observed that the convergence for 
the estimated state variables were slower. In the example involving velocity, the 
convergence was around 60s. The gains as a result of pole selection of [-0.3 -0.3 
-0.3 -0.1 -0.1 -0.1], were as follows. 


04 0 0 -0.03 0 0 
K,=| 0 -04 0] and K,=| 0 -003 0 
0 oO -04 0 0  -0.03 
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Velocity of target in X direction 
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Figure 13. Example of velocity estimation with lower gains. 


2. Filter Performance with Noisy Measurements 

The following graphs showed the estimation of target motion when zero 
mean white noise was added to the measurements. The rms in velocity channel 
was +2 m/s, camera pan/tilt rms was +0.3 degrees, SUAV euler angles rms was 
+ 2.8 degrees, image plane error Uc & v, rms was +5 degrees, and height z; rms 


from SUAV to target was +9 m. 
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Velocity of target in X direction 
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Figure 14. Estimated velocity in inertial frame (with measurement noise). 
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Mean of velocity error in X direction 
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Figure 15. Mean of target velocity error. 





Standard deviation of velocity error in X direction 
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Figure 16. Standard deviation of target velocity error. 
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Figure 17. Target position error (with measurement noise). 
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Mean of position error in X direction 
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Mean of position error in Z direction 














I I I I I 
| | | | | 
| | | | | 
| | | | | 
| é | | | 
a ee a a a a 
| | | | lL 
| | f | | 
| | | | " 
| | f | | 
Lis etl celle ees eae lias obits ote | 
| | | i | 
é | | | | 
| | f | | 
| E | | | 
E | | | | 
a ee ee ee ae a a a 
| | | | | 
| | | | | 
| | f | | 
| | | | | 
a 
| | | | | 
| | | | | 
| | | | | 
| | f | | 
| | | | | 
eS IS SS ee a 
| | f | | 
| | f | | 
| | c | | 
| | | | | 
fe St eo a hts aly eh a ne le ee Ss 
| | f | | 
E | | | | 
| | f | | 
| | | | 
E | | | | 
ee es i: a Dn ee | 
| | | | | 
| | | | a” 
| | f | | 
| | | | f 
Pee a hea lee Ete Oe cates os en eee ice 
| E | | q 
é f | | 1 
| | | { 
| | f | | 
f f | | Z 
Pe ee ee ee Se eel 
i { t | | 
| | | | | 
| | f | f 
| | | | | 
i i i i L 
wt oO) AN - oO - 
oO oO oO oO Q 


time (s) 


Figure 18. Mean of target position error. 
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Standard deviation of position error in X direction 
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Figure 19. Standard deviation of target position error. 
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Figure 20. 
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Figure 21. Estimated target heading (with measurement noise). 


The convergence for the estimates was now around 25 seconds for 
velocity (within 90% of true velocity), based on lower gains setting by selecting 
the poles [-0.3 -0.3 -0.3 -0.1 -0.1 -0.1 ]. Higher gains tended to cause wider 
fluctuations in the estimation of target motion. Thus, reduction in gains also has 


the effect of reducing the fluctuations in the estimation of target motion. 


In conclusion, this filter worked well in simulation in the presence of white 
noise. There was, however, a balance required between fast convergence time 
and degree of fluctuations in the target motion estimates. This can be achieved 
by selecting appropriate poles, hence the gains K, and Ko. 
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V. APPLICATION OF KALMAN FILTER 


A. DESCRIPTION OF FILTER 

The next filter, used to estimate target motion, employed the continuous 
nonlinear Kalman filtering technique described in Grewal et. al. [Ref 6]. Before 
the filter can be implemented, the kinematics of the tracking problem must be 
established. 

1. Kinematics Equations 

The following diagram showed the relations between the SUAV and 


moving target. 


SUAV 





target 


Figure 22. Target tracking in inertial frame [After: Ref 2]. 


The state variables x comprise the parameters 7, 9, 4, V, and w,. The 


target was assumed to be moving with constant velocity and heading. The 


resulting kinematics relationship was as follows: 


n=Z-AtW, [Ref 2] (8) 
n= Wa -A [Ref 2] (9) 
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p=-V,sinn+V,sin(y,+7-y,) [Ref 2] (10) 


_V,cosy V, cos(y, +7 —Wo) 
p p 


Zz (11) 


Letting V, =0, yw, =0 and assuming V, =0, we obtain 


V, sina ; Vy Sng V,cos7 . V,sin(v,+7-w,) . 


V,cos(y,+7-W,) . 
a 5 A+ 3 p 





p p p p p 
(12) 
V, =0 (13) 
yw, =0 (14) 
2. Process Model 


The nonlinear process model was obtained from Grewal et al [Ref 6] as 


follows: 
x(t) = f( x(t), t) + w(t) w(t) ~ N(0,Q(t)) (15) 
z(t) = h(x(t),t) + v(t) v(t) ~ N(O, A(t) (16) 
The implementation equations [Ref 6] were: 
X(t) = f(X(t),t) + K(t)[z(t) - 2(0)] (17) 


2(t) = h( X(t), t) (18) 


The linear approximation equations [Ref 6] were: 














Fj oe) (19) 
OX | x4) 
H(t) = oh( x, t) (20) 
OX | yx) 
The Kalman gain equations [Ref 6] were: 
P(t) = F(t)P(t) + P(t)F’ (t) + G(t)Q(t)G" (t) —K(t)R(t)K "(t) (21) 
K(t)= P(t)H"(t)R“(t) (22) 
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3. Kalman Gain Computation 
F(t) was computed based on the assumption of the following constant 


values: V,= 20 m/s, 7 =0, range p =500m, 2= Mf, = 0.04 rad/s, V, = 5 m/s, 
W, = 0.04 rad/s. y, took on the latest value from the SUAV truth model as the 


SUAV changes heading. y, was unknown and hence took on the value from 


latest estimated target heading y,. 


In the measurement equation (16), A(t) comprised measurements [7 V, ] 
where V, ~ pA. 


To obtain steady state gain, equation (21) was set to zero, i.e., letting 


P(t)=0. The gain from equation (22) was finally obtained by solving Algebraic 
Riccati Equation [Ref 2] for P. 


The process noise covariance Q and measurement noise covariance R 


were chosen as follows: 





[0.0001 0 0 0 0 
0 0.0001 0 0 0 
Q=| 0 0 0.0001 0 0 
0 0 0 0.0101 0 
| 0 0 0 0 0.0001] 
9 _[0:0005 0 
0 0.025 





Using nominal values, V,= 20 m/s, 7 = 0, range p = 500 m, A= Mo, = 


0.04 rad/s, V, = 5 m/s and yw, = 0.04 rad/s, the heading difference between the 


SUAV and target was varied over a cycle of 360 degrees. With these inputs to 


F(t), the steady state Kalman gain K was computed as described above and its 


values are shown below. 
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Figure 23. Kalman gain. 


UAV heading(degrees) 


In the above figure, the gain was denoted by kK, where the subscript / = 1 


to 5 was associated with the respective state variables 7, 9, 4, V, and y, in 


that order. The subscript / = 1 to 2 was associated with the measurements 77 and 


V, = pA respectively. Clearly, the gain varied according to the difference in 
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Equation (17) was implemented using existing simplified UAV truth model 


RESULTS AND DISCUSSION 


based on only one body turn rate y, in yaw, for the airframe. The filter 
0.1 


heading between SUAV and target. This set of gains was used to provide 


performance is shown in figures below. 


estimates of the state variables. 
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Figure 24. Estimated 77. 
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Figure 25. Estimated range from SUAV to target. 
The error was about 40m (after 500 seconds) compared to the true range 


of approximately 200m mean, meaning an error of about 20%. 
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Figure 26. Estimated target velocity V,. 


The target velocity estimate was not accurate enough. It was about twice 
the true target velocity based on a target velocity of [2, 2, 0] m/s in the inertial 
frame along xi, y; and Z; axis, i.e., 2.83 m/s. at a heading of 45 degrees or 0.785 
rad. The large discrepancy between the true and estimated velocity could not be 
successfully remedied. Possible causes could be due to the choice of nominal 
values assumed for variables in H(t) and the low values of the Kalman gain, for 
target velocity, which was related to the choice of noise covariance. 
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Figure 27. Estimated target heading y,. 


The mean heading error was about 0.06 rad average compared with the 


true heading of 0.785 rad. 


Overall, the filter in this particular implementation could provide estimates 


of the state variables to within 20% error approximately except for the target 


In future, further assessment using Kalman filter technique will be 


velocity. 


beneficial in identifying the cause of the estimation discrepancies observed 


above. 
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VI. CONCLUSION AND RECOMMENDATIONS 


In the tracking of stationary ground targets, previous work by various 
authors was discussed. With the background gathered from both the stationary 
and moving targets tracking, this project attempted to apply known filtering 
techniques to estimation of ground target range and velocity. In this thesis, the 
problem of tracking moving ground target using a camera mounted on a SUAV 


was assessed in simulation, using two different filters. 


The first filter was a nonlinear deterministic filter with stability guarantee. 
This technique was found to estimate the target motion with fast convergence in 
the region of five seconds, in the absence of measurement noise. With white 
noise in the measurement, the convergence time was around 25 seconds, using 
the gain described in this thesis. The estimates from this filter compared very well 
with the true target motion. 


The second filter technique assessed was based on the continuous 
nonlinear Kalman filter with steady state gain. This approach could not estimate 
closely the true target motion, for the case of this particular project, when 
compared with the first filter technique mentioned above. 


Overall, the attempt to estimate the moving ground target motion was 
successful using the first filtering technique. Future work is still required to verify 
the suitability of this filter using real flight test data. 


It is recommended for future work, that actual SUAV flight and target 
tracking data be used to verify the effectiveness of the nonlinear deterministic 
filter with stability guarantee. It is further recommended that the Kalman filtering 
technique be studied further to explore the issues, surrounding estimation of 
target motion, encountered in this project. 
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